﻿/*
 * lane_kalman.h
 *
 *  Created on: May 9, 2018
 *      Author: geoff
 */
#ifndef RMD_LANE_KALMAN_H_
#define RMD_LANE_KALMAN_H_

#include "data_frame.h"
#include "lane_const_parameter.h"
#include "lane_line_struct.h"
#include "lane_matrix.h"

namespace rmd {

// EKF struct
struct EKF_PointPara {
  CMatrix PredictState;
  CMatrix UpdateState;
  CMatrix BaseState;
  CMatrix NowState;
  CMatrix LastState;

  CMatrix PredictCovariance;
  CMatrix UpdateCovariance;
  CMatrix BaseCovariance;
  CMatrix NowConvariance;
  CMatrix G;

  CMatrix Z;
  CMatrix Z0;
  CMatrix R;

  CMatrix S;
  CMatrix Gain;

  double y_coord;

  bool flag_obsev;
  bool flag_first;
  bool flag_work;
  bool flag_is_good;
  bool flag_unconfirm;
  bool flag_stable;

  int time_no_obsev;
  int time_continu_obsev;
  int time_continu_good;
};

struct EKF_Para {
  EKF_PointPara point_bottom;
  EKF_PointPara point_middle;
  EKF_PointPara point_top;

  long index;

  LaneLine Line;

  float Midle_Near_Angle_Lable;
  bool Midle_Near_Angle_Flagwork;
  int Midle_Near_Angle_Cannotseetime;
};

class CKalman {
 private:
  EKF_Para line_ekf_[kNumEkf];

  CMatrix matrix_f_;
  CMatrix matrix_h_;

  float lane_width;

 public:
  // Define the system's own error when defining the system state transition
  // The better parameters for the straight line: define X1COV 10 X1VCOV 5 X2COV
  // 30  X2VCOV 3
  // Reduce the speed error to get a smoother result
  static const int kX1Cov;
  static const int kX1vCov;
  static const int kX2Cov;
  static const int kX2vCov;
  static const int kX3Cov;
  static const int kX3vCov;

  // Defining the errors caused by observations
  static const int kMx1Cov;
  static const int kMx2Cov;
  static const int kMx3Cov;
  static const int kMx4Cov;

  // Define error coefficients initialized using observations
  static const int kX1Covk;
  static const int kX1vCovk;
  static const int kX2Covk;
  static const int kX2vCovk;
  static const int kX3Covk;
  static const int kX3vCovk;

  // The number of consecutive observations after reaching continuous
  // observation
  // conditions is considered as stable
  static const int kStableTime;

  // Thresholds for jumping are considered
  static const int kJumpThread;

  // Think of a complete trace failure threshold
  static const int kErrorThread;

  // Aggregate bases that are considered good
  static const int kQualityTime;

  static const int kLinLooseMaxTime;

  static const int kContinuGood;

  CKalman();
  ~CKalman();

  static CKalman* instance_;
  static CKalman* Instance();

  std::vector<LaneLine> Update(DataFrame* data_frame,
                               std::vector<LaneLine>& now_lines);
  void ExcludeOutKalmanLinesByCurrent(std::vector<LaneLine>& tempResPriSaveLine);
  void EkfInitialization();

 private:
  void EkfMainProcess(EKF_PointPara& ekf_point, CMatrix _z_measure,
                      double _X_Cov_K, double _X_V_Cov_K);
  void GetEkfLineResult(EKF_Para& line_ekf);

  void EkfSingleParaInitialization(EKF_PointPara& point_ekf);
  void EkfProcessSingleLine(LaneLine now_line);

  double DoubleLineAngle(cv::Point2f line1_p1, cv::Point2f line1_p2,
                         cv::Point2f line2_p1, cv::Point2f line2_p2);
  double FindLinePara(cv::Point2f p1, cv::Point2f p2, double angle,
                      cv::Point2f p3, double y);

  void SetProcessNoiseCov(EKF_PointPara& ekf, double X_Cov, double X_V_Cov);
  void SetObservationNoiseCov(EKF_PointPara& ekf, double X_Cov);

  void Initialize(EKF_PointPara& point_ekf, double X_Cov_K, double X_V_Cov_K);

  void ObserveFromState(EKF_PointPara& point_ekf);
  void EkfPredict(EKF_PointPara& point_ekf);
  void EkfUpdate(EKF_PointPara& point_ekf);

  CMatrix SetValue4Cub(double x11, double x12, double x21, double x22);
  CMatrix SetValue2T(double x11, double x21);

  void SetObserve(EKF_PointPara& point_ekf, CMatrix z);


};
}
#endif
